/***************************************************************************
	This file has the miscellaneous functions required to run astar.c
	
	Copyright (C) 2013 Aditya Nag Matsa 
    
    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
****************************************************************************/

#include "astar.h"

/*************************************************
	Checks whether two Points are same or not
**************************************************/
int equalElements (Point *pt1, Point *pt2)
{
	if (pt1->x == pt2->x && pt1->y == pt2->y)
		return 1;
	else
		return 0;
}

/**************************************************
	Calculates distance between two points
***************************************************/
float distance (Point *pt1, Point *pt2)
{
	return sqrt ( (pt1->x - pt2->x)*(pt1->x - pt2->x) + (pt1->y - pt2->y)*(pt1->y - pt2->y) );
}

/**************************************************
	Calculates heuristic distance between two points
***************************************************/
float heuristic (Point *pt, Point *goal)
{
	float h_diagonal;
	
	if ( abs(pt->x-goal->x) < abs(pt->y-goal->y) )
		h_diagonal = abs(pt->x-goal->x);
	else
		h_diagonal = abs(pt->y-goal->y);
	
	float h_straight = abs(pt->x-goal->x) + abs(pt->y-goal->y);
	float h = sqrt(2) * h_diagonal + (h_straight - 2*h_diagonal);
	return h;
}

/**************************************************
	Finds all neighbor nodes of a point
***************************************************/
void neighborNodes (Point *pt, Point *nbd, FILE *map)
{
		char temp[8];
		
		fseek(map, (pt->x *(WIDTH+1)) + pt->y+1, SEEK_SET);
		temp[0] = getc(map);
		
		fseek(map, (pt->x+1)*(WIDTH+1) + pt->y+1, SEEK_SET);
		temp[1] = getc(map);
		
		fseek(map, (pt->x+1)*(WIDTH+1) + pt->y, SEEK_SET);
		temp[2] = getc(map);
		
		fseek(map, (pt->x+1)*(WIDTH+1) + pt->y-1, SEEK_SET);
		temp[3] = getc(map);
		
		fseek(map, pt->x*(WIDTH+1) + pt->y-1, SEEK_SET);
		temp[4] = getc(map);
		
		fseek(map, (pt->x-1)*(WIDTH+1) + pt->y-1, SEEK_SET);
		temp[5] = getc(map);
		
		fseek(map, (pt->x-1)*(WIDTH+1) + pt->y, SEEK_SET);
		temp[6] = getc(map);
		
		fseek(map, (pt->x-1)*(WIDTH+1) + pt->y+1, SEEK_SET);
		temp[7] = getc(map);
		
		if ( pt->y+1<WIDTH && temp[0] == ALLOW )
		{
			(nbd)->x = pt->x;
			(nbd)->y = pt->y+1;
		}
		else	
		{
			(nbd)->x = -1;
			(nbd)->y = -1;
		}
		
		if ( pt->x+1<LENGTH && pt->y+1<WIDTH && temp[1] == ALLOW )	
		{
			(nbd+1)->x = pt->x+1;
			(nbd+1)->y = pt->y+1;
		}
		else	
		{
			(nbd+1)->x = -1;
			(nbd+1)->y = -1;
		}
		
		if ( pt->x+1<LENGTH && temp[2] == ALLOW )	
		{
			(nbd+2)->x = pt->x+1;
			(nbd+2)->y = pt->y;
		}
		else
		{
			(nbd+2)->x = -1;
			(nbd+2)->y = -1;
		}
		
		if ( pt->x+1<LENGTH && pt->y-1>=0 && temp[3] == ALLOW )	
		{
			(nbd+3)->x = pt->x+1; 
			(nbd+3)->y = pt->y-1;
		}
		else
		{
			(nbd+3)->x = -1; 
			(nbd+3)->y = -1;
		}
		
		if ( pt->y-1>=0 && temp[4] == ALLOW )	
		{
			(nbd+4)->x = pt->x; 
			(nbd+4)->y = pt->y-1;
		}
		else
		{
			(nbd+4)->x = -1; 
			(nbd+4)->y = -1;
		}
		
		if ( pt->x-1>=0 && pt->y-1>=0 && temp[5] == ALLOW )	
		{
			(nbd+5)->x = pt->x-1;
			(nbd+5)->y = pt->y-1;
		}
		else
		{
			(nbd+5)->x = -1;
			(nbd+5)->y = -1;
		}
		
		if ( pt->x-1>=0 && temp[6] == ALLOW )	
		{
			(nbd+6)->x = pt->x-1;
			(nbd+6)->y = pt->y;
		}
		else
		{
			(nbd+6)->x = -1;
			(nbd+6)->y = -1;
		}
		
		if ( pt->x-1>=0 && pt->y+1<WIDTH &&	temp[7] == ALLOW )	
		{
			(nbd+7)->x = pt->x-1;
			(nbd+7)->y = pt->y+1;
		}
		else
		{
			(nbd+7)->x = -1;
			(nbd+7)->y = -1;
		}
}

/***************************************************************
	Reconstructs the path from goal to start
****************************************************************/
void reconstructPath (Point *pt, FILE *fp)
{
	while ( pt != NULL )
	{
		fseek( fp, (pt->x * (WIDTH+1)) + pt->y, SEEK_SET);
		fputc(PATH, fp);
		pt = pt->from;
	}
}
/***************************************************************
	Stores the set in a file
****************************************************************/
void print(Node *set, FILE *fp)
{
	while(set != NULL)
	{
		fprintf(fp, "%d %d %f %f\n", set->location->x, set->location->y, set->location->g, set->location->f);
		set = set->next;
	}
}
